package edu.arizona.sista.learn.model.motion;

import java.io.File;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.Scanner;

import edu.arizona.sista.learn.model.Interval;
import edu.arizona.sista.learn.model.holding.Person;
import edu.arizona.sista.learn.model.holding.Scene;
import edu.arizona.sista.learn.model.inferences.Predicate;

public class Speed {
	public static double MAX_SPEED_VARIATION = 0.05;
	public static double MIN_FRAMES = 10;
	public static int ACCEL = 2;
	public static int CONST = 1;
	public static int DECEL = 0;
	public static int MLESS = 9;
	public static int CHUNK_SIZE = 4;
	private static int X = 0;
	private static int Y = 1;
	private static double MIN_SPEED_X = Double.MAX_VALUE;
	private static double MIN_SPEED_Y = Double.MAX_VALUE;
	private static int LAST_LOCATION_ID = 1;
	
	public static void speed(Scene scene, boolean handHeld) {
		
		//computeWeights(scene);
		
		double minSpeedX, minSpeedY;
		int w1, w2, w3, w4;
		
		if (handHeld){
			minSpeedX = 0.001;
			minSpeedY = 0.041;
			w1 = 1400;
			w2 = 100;
			w3 = 35;
			w4 = 1;
		}
		else{
			minSpeedX = 0.014;
			minSpeedY = 0.008;
			w1 = 100;
			w2 = 100;
			w3 = 35;
			w4 = 1;
		}

		//get granted rectilinear trajectories
		List<RectilinearTrajectory> rt = scene.getRectilinearTrajectories();
		
		for(Person p: scene._people){
			//compute movement fluent
			double[] fluent = smooth(insertLinearTrajectories(speedFluent(p._tracks, w1, w2, w3, w4, minSpeedX, minSpeedY), p, rt));

			//add movements to scene
			int i = 0;
			while(i < fluent.length){
				while(i < fluent.length && fluent[i] < .5) i++;
				int beginIndex = i * CHUNK_SIZE;
				if (beginIndex == fluent.length) continue;
				while(i < fluent.length && fluent[i] >= .5) i++;
				int endIndex = i * CHUNK_SIZE;
				if (beginIndex < endIndex) scene.addEvent(new MovementEvent(p, new Interval(beginIndex, endIndex)));
			}
			
			List<MovementEvent> me = scene.getMovementEvents();
			
			List<Interval> sl = deduceMotionLessIntervals(me, p);
			for(Interval t: sl){
				String st = "t_"  + t.start() + "-" + t.end();
				scene.addPredicate(new Predicate("isa(" + st + ", TimeInterval)"));
				scene.addPredicate(new Predicate("isa(l_" + LAST_LOCATION_ID + ", Location)"));
				scene.addPredicate(new Predicate("holdsIn(" + st + ", locatedAt(" + p.getVariableName() + ", l_" + LAST_LOCATION_ID + ")))"));
				LAST_LOCATION_ID++;
			}
		}
	}
	
	private static List<Interval> deduceMotionLessIntervals(List<MovementEvent> list, Person p) {
		List<Interval> output = new ArrayList<Interval>();
		int lastEndIndex = minFrame(p._tracks);
		for(MovementEvent me: list){
			if (me._doer.equals(p)){
				if (me._time.start() > lastEndIndex) output.add(new Interval(lastEndIndex, me._time.start() - 1));
				lastEndIndex = me._time.end() + 1;
			}
		}
		int maxFrame = maxFrame(p._tracks);
		if(lastEndIndex < maxFrame) output.add(new Interval(lastEndIndex + 1, maxFrame));
		return output;
	}

	/**
	 * Brute force training method to compute optimized weights
	 * @param scene
	 */
	
	private static void computeWeights(Scene scene) {
		//get granted rectilinear trajectories
		List<RectilinearTrajectory> rt = scene.getRectilinearTrajectories();
		
		double errorMin = Double.MAX_VALUE;
		
		//input file and number of frames set by hand! (sorry!)
		Map<Person, double[]> perfectFluents = load("data/3d-tracks/newspaper/perfect-fluents.csv", scene, 167);
		
		for (CHUNK_SIZE = 3; CHUNK_SIZE <= 10; CHUNK_SIZE++){
		for (double minSpeedX = 0; minSpeedX < .01; minSpeedX += .0005){
		for (double minSpeedY = 0; minSpeedY < .01; minSpeedY += .0005){
		for (int w1 = 50; w1 < 5000; w1 += 50){
		for (int w2 = 50; w2 < 5000; w2 += 50){
		for (int w3 = 1; w3 <= 100; w3 += 1){
		for (int w4 = 1; w4 <= 1000; w4 += 1){
			double error = 0;
			int sum = 0;
			for (Person p: scene._people){
				double[] fluent = smooth(insertLinearTrajectories(speedFluent(p._tracks, w1, w2, w3, w4, minSpeedX, minSpeedY), p, rt));
				double[] perfect = toChunks(perfectFluents.get(p));
				sum += perfect.length;
				for (int i = 0; i < fluent.length; i++) error += Math.abs(fluent[i] - perfect[i]);
			}
			error = error * CHUNK_SIZE;
			sum = sum * CHUNK_SIZE;
			if (error < errorMin){
				errorMin = error;
				System.out.println("Current best configuration:");
				System.out.println("Chunk size: " + CHUNK_SIZE);
				System.out.println("Min speed x: " + minSpeedX);
				System.out.println("Min speed y: " + minSpeedY);
				System.out.println("w1: " + w1);
				System.out.println("w2: " + w2);
				System.out.println("w3: " + w3);
				System.out.println("w4: " + w4);
				System.out.println("Error: " + errorMin + " / " + sum);
			}
		}}}}}}}
	}

	private static double[] insertLinearTrajectories(double[] fluent, Person p, List<RectilinearTrajectory> rt) {
		int n = 0;
		for(RectilinearTrajectory t: rt){
			if (t._doer.equals(p)){
				for(Track track: t._tracks){
					int frame = track.frame;
					if (frame > n) n =frame;
				}
			}
		}
		
		//build fluent
		double[] rtFluent = new double[n + 1];
		for(RectilinearTrajectory t: rt){
			if (t._doer.equals(p)){
				for(Track track: t._tracks){
					int frame = track.frame;
					rtFluent[frame] = 1;
				}
			}
		}
		
		int m = (int) Math.ceil((double) n / CHUNK_SIZE);
		double[] chunks = toChunks(rtFluent);
		
		for(int i = 0; i < m; i++) if (chunks[i] >= .5) fluent[i] = 1;
		return fluent;
	}
	
	private static Map<Person, double[]> load(String file, Scene scene, int maxFrame) {
		Map<Person, double[]> output = new HashMap<Person, double[]>();
		try {
			Scanner scanner = new Scanner(new File(file));
			int lastId = -1;
			Person p = null;
			double[] fluent = null;
			
			//skip first line
			scanner.nextLine();

			while (scanner.hasNextLine()) {
			    String line = scanner.nextLine();
			    
			    //0 id
			    //1 label
			    //2 begin
			    //3 end
			
			    // Break comma separated line using ","
				String[] tokens = line.split(",");
			    
				int id = Integer.parseInt(tokens[0]);
				String label = tokens[1];
				int begin = Integer.parseInt(tokens[2]);
				int end = Integer.parseInt(tokens[3]);
				
				if (id != lastId){
					if (p != null){
						double[] copy = new double[fluent.length];
						System.arraycopy(fluent, 0, copy, 0, fluent.length);
						output.put(p, copy);
					}
					
					//new person
					p = scene.getPerson(label);
					fluent = new double[maxFrame + 1];
				}
					
				for (int i = begin; i <= end; i++) fluent[i] = 1;
				lastId = id;			
			}
			double[] copy = new double[fluent.length];
			System.arraycopy(fluent, 0, copy, 0, fluent.length);
			output.put(p, copy);

			scanner.close();
		} catch (Exception e) {
			System.out.println("Exception while reading csv file: " + e);
			e.printStackTrace();
		}		
		return output;
	}

	private static double[] toChunks(double[] fluent) {
		int n = fluent.length;
		int m = (int) Math.ceil((double) n / CHUNK_SIZE);
		double[] chunks = new double[m];
		for(int i = 0; i < m; i++){
			int indexMax = CHUNK_SIZE;
			if (i == m - 1) indexMax = n - i * CHUNK_SIZE;
			double sum = 0;
			for(int j = 0; j < indexMax; j++) sum += fluent[i * CHUNK_SIZE + j];
			double mean = sum / indexMax;
			chunks[i] = mean;
		}
		return chunks;
	}

	private static double[] speedFluent(List<Track> tracks, int w1, int w2, int w3, int w4, double minSpeedX, double minSpeedY) {
		double[] fluentX = speedFluent(tracks, w1, minSpeedX, X);
		double[] fluentY = speedFluent(tracks, w2, minSpeedY, Y);
		double[] fluentA = angleFluent(tracks, w3, w4);
		
		double[] motionFluent = new double[fluentX.length];
		for(int i = 0; i < motionFluent.length; i++) motionFluent[i] = fluentX[i] * fluentY[i] * fluentA[i];
		
		return motionFluent;
	}
	
	/**
	 * Probabilistic fluent based on direction changes
	 * @param tracks
	 * @param w3
	 * @param w4
	 * @return
	 */
	
	private static double[] angleFluent(List<Track> tracks, int w3, int w4) {
		int n = tracks.get(tracks.size() - 1).frame + 1;
		int chunks = (int) Math.ceil((double) n / CHUNK_SIZE);
		double[] fluent = new double[chunks];
		for(int i = 0; i < chunks; i++){
			int indexMax = (i + 1) * CHUNK_SIZE;
			if (i == chunks - 1) indexMax = n;
			List<Integer> indexes = indexes(tracks, i * CHUNK_SIZE, indexMax);
			if (indexes.size() > 2){
				double[] p = tracks.get(indexes.get(0)).pos;
				double[] q = tracks.get(indexes.get(1)).pos;
				double[] lastVector = new double[]{q[0] - p[0], q[1] - p[1]};
				double lastNorm = Math.sqrt(Math.pow(lastVector[0], 2) + Math.pow(lastVector[1], 2));
				double sum = 0;
				for(int j = 2; j < indexes.size(); j++){
					p = tracks.get(indexes.get(j - 1)).pos;
					q = tracks.get(indexes.get(j)).pos;
					double[] vector = new double[]{q[0] - p[0], q[1] - p[1]};
					double norm = Math.sqrt(Math.pow(vector[0], 2) + Math.pow(vector[1], 2));
					double scalar = lastVector[0] * vector[0] + lastVector[1] * vector[1];
					double angle = Math.abs(Math.acos(scalar / (lastNorm * norm)));
					if (Double.isNaN(angle)) angle = 0;
					sum += angle;
					lastNorm = norm;
					lastVector = vector;
				}
				double mean = sum / (indexes.size() - 2);
				double value = (Math.cos(Math.pow(mean, w3) / (Math.pow(Math.PI, w3 - 1))) + w4) / (w4 + 1);
				fluent[i] = value;
			}
			else fluent[i] = 1;
		}
		return fluent;
	}

	/**
	 * Get the indexes that correspond to frames in tracks
	 * @param tracks
	 * @param beginIndex
	 * @param endIndex
	 * @return 
	 */
	private static List<Integer> indexes(List<Track> tracks, int beginIndex, int endIndex) {
		List<Integer> output = new ArrayList<Integer>();
		int i = 0;
		while (i < tracks.size() && tracks.get(i).frame < beginIndex) i++;
		while (i < tracks.size() && tracks.get(i).frame < endIndex){
			output.add(i);
			i++;
		}
		return output;
	}

	/**
	 * Probabilistic fluent based on movements on one axis
	 * @param tracks
	 * @param w
	 * @param minSpeed
	 * @param axis
	 * @return
	 */
	private static double[] speedFluent(List<Track> tracks, int w, double minSpeed, int axis) {
		int n = maxFrame(tracks);
		int chunks = (int) Math.ceil((double) n / CHUNK_SIZE);
		double[] fluent = new double[chunks];
		int trackIndex = 0; 
		for(int i = 0; i < chunks && trackIndex < tracks.size(); i++){
			int indexMax = (i + 1) * CHUNK_SIZE;
			if (i == chunks - 1) indexMax = n;
			int beginIndex = tracks.get(trackIndex).frame;
			double beginPos;
			if (axis == X) beginPos = tracks.get(trackIndex).pos[0];
			else beginPos = tracks.get(trackIndex).pos[1];
			if (beginIndex <= indexMax){
				while(tracks.get(trackIndex).frame < indexMax) trackIndex++;
				if (trackIndex > 0){
					int endIndex = tracks.get(trackIndex - 1).frame;
					if (beginIndex != endIndex){
						double endPos;
						if (axis == X) endPos = tracks.get(trackIndex - 1).pos[0];
						else endPos = tracks.get(trackIndex - 1).pos[1];
						double speed = Math.abs((endPos - beginPos) / (endIndex - beginIndex));
						if (speed > minSpeed) fluent[i] = 1;
						else fluent[i] = Math.exp(- Math.pow(w * (speed - minSpeed), 2));
						if (axis == X) if (speed < MIN_SPEED_X) MIN_SPEED_X = speed;
						if (axis == Y) if (speed < MIN_SPEED_Y) MIN_SPEED_Y = speed;
					}
				}
			}
		}
		return fluent;
	}

	private static int minFrame(List<Track> tracks) {
		int min = Integer.MAX_VALUE;
		for(Track t: tracks) if (t.frame < min) min = t.frame;
		return min;
	}

	private static int maxFrame(List<Track> tracks) {
		int max = 0;
		for(Track t: tracks) if (t.frame > max) max = t.frame;
		return max;
	}

	private static double[] smooth(double[] fluent) {
		//rules:
		//if a gap between two movements does not last longer than MIN_FRAMES then fill the gap
		//if a movement does not last longer than MIN_FRAMES then remove it
		int n = fluent.length;
		
		//Rule 1
		int i = 0;
		while(i < n){
			while(i < n && fluent[i] < .5) i++;
			while(i < n && fluent[i] >= .5) i++;
			int beginIndex = i;
			while(i < n && fluent[i] < .5) i++;
			int endIndex = i;
			if (i < n && (endIndex - beginIndex) * CHUNK_SIZE < MIN_FRAMES){
				double incr = (fluent[endIndex] - fluent[beginIndex - 1]) / (endIndex - beginIndex + 1);
				for(int j = beginIndex; j < endIndex; j++){
					fluent[j] = fluent[j - 1] + incr;
				}
			}
		}
		
		//Rule 2
		i = 0;
		while(i < n){
			while(i < n && fluent[i] >= .5) i++;
			while(i < n && fluent[i] < .5) i++;
			int beginIndex = i;
			while(i < n && fluent[i] >= .5) i++;
			int endIndex = i;
			if (i < n && (endIndex - beginIndex) * CHUNK_SIZE < MIN_FRAMES){
				double incr = (fluent[endIndex] - fluent[beginIndex - 1]) / (endIndex - beginIndex + 1); 
				for(int j = beginIndex; j < endIndex; j++){
					fluent[j] = fluent[j - 1] + incr;
				}
			}
		}
		
		return fluent;
	}
}
